#include "ak_common.h"
#include "AnyKaDrv.h"
#include "SdkMotor.h"

#define  UP_AND_DOWN_GROUP       4
#define  LEFT_AND_RIGHT_GROUP    5

#define  CENTER_POSITION_VALUE                (98 * 100)
#define  UP_DOWN_CENTER_POSITION_VALUE        (135 * 100)

#define  CTRL_UP_DOWN_MAX_VALUE               (143 * 100)
#define  CTRL_UP_DOWN_MIN_VALUE               (115 * 100)
#define  CTRL_LEFT_RIGHT_MAX_VALUE            (126 * 100)
#define  CTRL_LEFT_RIGHT_NIN_VALUE            (70 * 100)

void *g_AKSdk_u_and_d_handle = NULL;
void *g_AKSdk_l_and_r_handle = NULL;

void *ak_pwm_open(int d_value)
{
    void *handle = NULL;
    handle = ak_drv_pwm_open(d_value);
    if(!handle)
    {
        ak_print_error("open pwm device fail.\n");
        return NULL;
    }

    return handle;
}

void ak_pwm_close(void *handle)
{
    ak_drv_pwm_close(handle);
}

/* d_value is pwm group, num_value is angle*/
int ak_pwm_ctrl(int d_value,int num_value)
{
    int ret = 0;
    void *handle = NULL;

    if (d_value == LEFT_AND_RIGHT_GROUP)
    {
        handle = g_AKSdk_l_and_r_handle;
        num_value = num_value < CTRL_LEFT_RIGHT_NIN_VALUE ? CTRL_LEFT_RIGHT_NIN_VALUE : num_value;
        num_value = num_value > CTRL_LEFT_RIGHT_MAX_VALUE ? CTRL_LEFT_RIGHT_MAX_VALUE : num_value;
    }
    else if (d_value == UP_AND_DOWN_GROUP)
    {
        handle = g_AKSdk_u_and_d_handle;
        num_value = num_value < CTRL_UP_DOWN_MIN_VALUE ? CTRL_UP_DOWN_MIN_VALUE : num_value;
        num_value = num_value > CTRL_UP_DOWN_MAX_VALUE ? CTRL_UP_DOWN_MAX_VALUE : num_value;
    }

    ret = ak_drv_pwm_set(handle, 100, num_value, 65536);
    if(ret < 0)
    {
        ak_print_error("set pwm frq duty cycle fail.\n");
        return -1;
    }

    return 0;
}

void Ext_SdkMotor_Init(void)
{
    g_AKSdk_l_and_r_handle = ak_pwm_open(LEFT_AND_RIGHT_GROUP);
    if (!g_AKSdk_l_and_r_handle)
    {
        ak_print_error("open l and r handle fail.\n");
        return;
    }

    g_AKSdk_u_and_d_handle = ak_pwm_open(UP_AND_DOWN_GROUP);
    if (!g_AKSdk_u_and_d_handle)
    {
        ak_print_error("open u and d handle fail.\n");
        return;
    }

    return;
}

void Ext_SdkMotor_Wait_Stop(int num)
{
}

void Ext_SdkMotor_TurnLeft(int num)
{
}

void Ext_SdkMotor_TurnRight(int num)
{
}

void Ext_SdkMotor_TurnVal(int num,int val)
{
}

void Ext_SdkMotor_Stop(int num)
{
}

void Ext_SdkMotor_Init_Center(int num)
{
    ak_print_normal("init motor position\n");
    ak_pwm_ctrl(LEFT_AND_RIGHT_GROUP,CENTER_POSITION_VALUE);
    ak_pwm_ctrl(UP_AND_DOWN_GROUP,UP_DOWN_CENTER_POSITION_VALUE);
}

void Ext_SdkMotor_Destroy(void)
{
    ak_pwm_close(g_AKSdk_l_and_r_handle);
    ak_pwm_close(g_AKSdk_u_and_d_handle);
}
